#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static int _sign(double d) {
    if (d > 0) {
        return 1;
    }
    if (d < 0) {
        return -1;
    }
    return 0;
}

static double _match_shapes(const std::vector<cv::Point>& contourA, const std::vector<cv::Point>& contourB, int method) {
    double huA[7], huB[7];
    cv::HuMoments(cv::moments(contourA), huA);
    cv::HuMoments(cv::moments(contourB), huB);
    
    bool anyA = false, anyB = false;
    double eps = 1.e-5;
    double result = 0;
    switch (method) {
        case cv::CONTOURS_MATCH_I1:
            for (int i = 0; i < 7; i++) {
                double aHuA = std::fabs(huA[i]);
                double aHuB = std::fabs(huB[i]);
                if (aHuA > 0) {
                    anyA = true;
                }
                if (aHuB > 0) {
                    anyB = true;
                }
                int signHuA = _sign(huA[i]);
                int signHuB = _sign(huB[i]);
                if (aHuA > eps && aHuB > eps) {
                    double mA = signHuA * std::log10(aHuA);
                    double mB = signHuB * std::log10(aHuB);
                    result += std::fabs(1./mA - 1./mB);
                }
            }
            break;
        case cv::CONTOURS_MATCH_I2:
            for (int i = 0; i < 7; i++) {
                double aHuA = std::fabs(huA[i]);
                double aHuB = std::fabs(huB[i]);
                if (aHuA > 0) {
                    anyA = true;
                }
                if (aHuB > 0) {
                    anyB = true;
                }
                int signHuA = _sign(huA[i]);
                int signHuB = _sign(huB[i]);
                if (aHuA > eps && aHuB > eps) {
                    double mA = signHuA * std::log10(aHuA);
                    double mB = signHuB * std::log10(aHuB);
                    result += std::fabs(mA - mB);
                }
            }
            break;
        case cv::CONTOURS_MATCH_I3:
            for (int i = 0; i < 7; i++) {
                double aHuA = std::fabs(huA[i]);
                double aHuB = std::fabs(huB[i]);
                if (aHuA > 0) {
                    anyA = true;
                }
                if (aHuB > 0) {
                    anyB = true;
                }
                int signHuA = _sign(huA[i]);
                int signHuB = _sign(huB[i]);
                if (aHuA > eps && aHuB > eps) {
                    double mA = signHuA * std::log10(aHuA);
                    double mB = signHuB * std::log10(aHuB);
                    double temp = std::fabs(mA-mB) / std::fabs(mA);
                    if (result < temp) {
                        result = temp;
                    }
                }
            }
            break;
        default:
            CV_Error(1, "Unknown method!");
    }
    
    if (anyA != anyB) {
        result = DBL_MAX;
    }
    return result;
}

int main(int argc, char **argv) {
    cv::Mat mat1 = cv::Mat::zeros(cv::Size(80, 100), CV_8UC1);
    cv::Mat mat2 = mat1.clone();
    cv::Mat mat3 = mat1.clone();
    
    std::vector<cv::Point> c1, c2, c3;
    c1.push_back(cv::Point(40, 10));
    c1.push_back(cv::Point(30, 35));
    c1.push_back(cv::Point(10, 50));
    c1.push_back(cv::Point(30, 65));
    c1.push_back(cv::Point(40, 90));
    c1.push_back(cv::Point(50, 65));
    c1.push_back(cv::Point(70, 50));
    c1.push_back(cv::Point(50, 35));
    
    std::vector<std::vector<cv::Point>> contours;
    contours.push_back(c1);
    cv::drawContours(mat1, contours, 0, cv::Scalar::all(255), -1);
    
    cv::Mat m = cv::getRotationMatrix2D(cv::Point2f(40, 50), -45, 1);
    cv::warpAffine(mat1, mat2, m, mat1.size());
    std::vector<std::vector<cv::Point>> contours2;
    cv::findContours(mat2, contours2, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
    CV_Assert(contours2.size() == 1);
    c2 = contours2[0];
    contours.push_back(c2);
    
    c3.push_back(cv::Point(20, 20));
    c3.push_back(cv::Point(20, 80));
    c3.push_back(cv::Point(60, 80));
    c3.push_back(cv::Point(60, 20));
    contours.push_back(c3);
    cv::drawContours(mat3, contours, 2, cv::Scalar::all(255), -1);
    
    std::vector<int> methods = {cv::CONTOURS_MATCH_I1, cv::CONTOURS_MATCH_I2, cv::CONTOURS_MATCH_I3};
    std::vector<cv::String> methodNames = {"I1", "I2", "I3"};
    for (int i = 0; i < methods.size(); i++) {
        std::cout << methodNames[i] << ": " << std::endl;
        for (int j = 0; j < contours.size(); j++) {
            for (int k = j; k < contours.size(); k++) {
                double similarity = cv::matchShapes(contours[j], contours[k], methods[i], 0);
                std::cout << "\t" << (j+1) << " vs " << (k+1) << ": similarity = " << similarity << std::endl;
                similarity = _match_shapes(contours[j], contours[k], methods[i]);
                std::cout << "\t" << (j+1) << " vs " << (k+1) << ": similarity = " << similarity << std::endl;
                std::cout << "\t" << "----------------------------------------------------" << std::endl;
            }
        }
    }
    
    cv::imshow("mat1", mat1);
    cv::imshow("mat2", mat2);
    cv::imshow("mat3", mat3);
    
    cv::waitKey(0);
    
    return 0;
}
